/**
 ******************************************************************************
 * @FilePath: \for_infantry\App\nuc_interface.c
 * @Brief: 
 * @Date: 2021-01-21 18:56:17
 * @Author: Rio
 ******************************************************************************
 */
#include "nuc_interface.h"
#include "usart.h"
#include "bsp_usart.h"
#include "MonitorTask.h"
#include "Gimbal.h"
#include "user_lib.h"
#include "CRC_Check.h"
#include "Motor_control.h"
#include "kalman_filter.h"
#include "DebugTask.h"
#include "STMGood.h"
#include <string.h>
#include <math.h>

extern DMA_HandleTypeDef hdma_usart6_rx;
extern DMA_HandleTypeDef hdma_usart6_tx;

#define VISION_RX_USART             USART6
#define VISION_RX_USART_HANDLE		  huart6
#define VISION_DMA_USART            hdma_usart6_rx

typedef struct  {
    float fdata[2];
    unsigned char tail[4];
}Frame;

Frame send;

extern OFFLINE_COUNT_STRUCT Fps;
extern visInf_t s_visionInform;
extern motorData_t s_gimbal_pit_motor, s_gimbal_yaw_motor;
extern float     yawSpd_buff[10];
extern float S2;
extern double X, Y;
uint8_t visionData_buff[NUC_REC_LEN];
uint8_t sendBuf[30];
float dtaX_kf = 0.0f,dtaX = 0.0f, visImu = 0.0f, visX;
float lastX = 0.0f, lastY = 0.0f, lastZ= 0.0f;
PC_DATA pcData;
filterLow_t s_pitFilter, s_pitOffset, s_yawFilter;
kalman1_state s_yawKalmanFil;
kalman1_state s_dtaxKalmanFil;

static void VofaJF(Frame *vofaData, float a, float b);

static const unsigned char crcTable[] =
{
    0x00,0x31,0x62,0x53,0xc4,0xf5,0xa6,0x97,0xb9,0x88,0xdb,0xea,0x7d,0x4c,0x1f,0x2e,
    0x43,0x72,0x21,0x10,0x87,0xb6,0xe5,0xd4,0xfa,0xcb,0x98,0xa9,0x3e,0x0f,0x5c,0x6d,
    0x86,0xb7,0xe4,0xd5,0x42,0x73,0x20,0x11,0x3f,0x0e,0x5d,0x6c,0xfb,0xca,0x99,0xa8,
    0xc5,0xf4,0xa7,0x96,0x01,0x30,0x63,0x52,0x7c,0x4d,0x1e,0x2f,0xb8,0x89,0xda,0xeb,
    0x3d,0x0c,0x5f,0x6e,0xf9,0xc8,0x9b,0xaa,0x84,0xb5,0xe6,0xd7,0x40,0x71,0x22,0x13,
    0x7e,0x4f,0x1c,0x2d,0xba,0x8b,0xd8,0xe9,0xc7,0xf6,0xa5,0x94,0x03,0x32,0x61,0x50,
    0xbb,0x8a,0xd9,0xe8,0x7f,0x4e,0x1d,0x2c,0x02,0x33,0x60,0x51,0xc6,0xf7,0xa4,0x95,
    0xf8,0xc9,0x9a,0xab,0x3c,0x0d,0x5e,0x6f,0x41,0x70,0x23,0x12,0x85,0xb4,0xe7,0xd6,
    0x7a,0x4b,0x18,0x29,0xbe,0x8f,0xdc,0xed,0xc3,0xf2,0xa1,0x90,0x07,0x36,0x65,0x54,
    0x39,0x08,0x5b,0x6a,0xfd,0xcc,0x9f,0xae,0x80,0xb1,0xe2,0xd3,0x44,0x75,0x26,0x17,
    0xfc,0xcd,0x9e,0xaf,0x38,0x09,0x5a,0x6b,0x45,0x74,0x27,0x16,0x81,0xb0,0xe3,0xd2,
    0xbf,0x8e,0xdd,0xec,0x7b,0x4a,0x19,0x28,0x06,0x37,0x64,0x55,0xc2,0xf3,0xa0,0x91,
    0x47,0x76,0x25,0x14,0x83,0xb2,0xe1,0xd0,0xfe,0xcf,0x9c,0xad,0x3a,0x0b,0x58,0x69,
    0x04,0x35,0x66,0x57,0xc0,0xf1,0xa2,0x93,0xbd,0x8c,0xdf,0xee,0x79,0x48,0x1b,0x2a,
    0xc1,0xf0,0xa3,0x92,0x05,0x34,0x67,0x56,0x78,0x49,0x1a,0x2b,0xbc,0x8d,0xde,0xef,
    0x82,0xb3,0xe0,0xd1,0x46,0x77,0x24,0x15,0x3b,0x0a,0x59,0x68,0xff,0xce,0x9d,0xac
};

static unsigned char calCrcTable(unsigned char *ptr, unsigned char len)
{
  unsigned char  crc = 0x00;

  while (len--)
  {
      crc = crcTable[crc ^ *ptr++];
  }
  return (crc);
}


void sendMessageToPc(uint8_t selectRB, uint8_t selectMode)
{
    uint8_t pcTxBuf[3];
    pcTxBuf[0] = selectRB;// 0->red attackBlue | 1->blue attackRed
    pcTxBuf[1] = selectMode;
    pcTxBuf[2] = '\n';
    HAL_UART_Transmit(&VISION_RX_USART_HANDLE, pcTxBuf, 3, 100);
}

void sendDataToNuc(int8_t loop,int8_t id)
{
	static int8_t tic;
	tic++;
	if(tic >= loop)
	{
		tic = 0;
		uint8_t mcuTxBuf[6];
		mcuTxBuf[0] = 0xA6;
		mcuTxBuf[1] = id;
		mcuTxBuf[2] = 0;
		Append_CRC8_Check_Sum(mcuTxBuf,4);
		mcuTxBuf[4] = '\r';
		mcuTxBuf[5] = '\n';
		HAL_UART_Transmit(&VISION_RX_USART_HANDLE, mcuTxBuf, 6, 100);
	}
}
	

/**
  * @brief   The callback function for remote control
  * @param   none
  * @retval  None
  */
void DealPcData(PC_DATA *pcdata, uint8_t *visionBuf)
{
	static int32_t fps;
	pcdata->checkCode.bit[0] = visionBuf[14];
	pcdata->checkCode.bit[1] = visionBuf[15];
	pcdata->checkCodeCal = calCrcTable(visionBuf, 13);

    if(visionBuf[0] == 0xA5 && pcdata->checkCode.data == pcdata->checkCodeCal)
    {
//        printf("CRC OK\r\n");
	    {
        pcdata->x.bit[0] = visionBuf[2];
        pcdata->x.bit[1] = visionBuf[3];
        pcdata->x.bit[2] = visionBuf[4];
        pcdata->x.bit[3] = visionBuf[5];
        pcdata->y.bit[0] = visionBuf[6];
        pcdata->y.bit[1] = visionBuf[7];
        pcdata->y.bit[2] = visionBuf[8];
        pcdata->y.bit[3] = visionBuf[9];
        pcdata->z.bit[0] = visionBuf[10];
        pcdata->z.bit[1] = visionBuf[11];
        pcdata->z.bit[2] = visionBuf[12];
        pcdata->z.bit[3] = visionBuf[13];
//				printf("%d %f\r\n",fps, lastX);
				if(pcdata->z.data < 0.0f)
				{
					fps++;
					if(fps <= 3) 
					{
						pcdata->x.data = lastX;
						pcdata->y.data = lastY;
						pcdata->z.data = lastZ;
					}
					else 
					{
						pcdata->x.data = -1.0;
						pcdata->y.data = -1.0;
//						fps = 0;
					}
				}
				else
				{
					fps = 0;
				}

				s_visionInform.yawPos = pcdata->x.data;
				s_visionInform.pitPos = pcdata->y.data;
				s_visionInform.distance = LowFreFilter(&s_pitOffset, pcdata->z.data);
		
			}
			
			{
				s_visionInform.pitchOffset = -0.65*(s_visionInform.distance - 300) \
																			-2.3*(s_visionInform.distance*sinf(s_gimbal_pit_motor.gyro_angle*3.14159/180)\
																			-4.9*(s_visionInform.distance/1535)*(s_visionInform.distance/1535));
				s_gimbal_pit_motor.visPitTarget = s_visionInform.pitPos + 0.4 * s_visionInform.pitchOffset;
			}
			
			{
				visImu = yawSpd_buff[2];
				dtaX = pcdata->x.data - lastX;
//				dtaX_kf = kalman1_filter(&s_dtaxKalmanFil,dtaX);
				visX = 1.9 * dtaX;//3.65
					if(visX > 500.0f) visX = visImu;
//				printf("%f,%f\r\n", dtaX, visImu);
//				printf("%f\r\n",visX);
				s_visionInform.yawRelV = visX - visImu;//kalman1_filter(&s_yawKalmanFil,(visX - visImu));
				if(fabs(s_visionInform.yawRelV) > 1000.0f ) s_visionInform.yawRelV *= 0.000001;
				
				s_visionInform.yawPre = s_visionInform.yawRelV * 66;//(0.12*s_visionInform.distance+24.0);
				
				if(fabs(s_visionInform.yawPre) > 0 && fabs(s_visionInform.yawPre) < 600)
					s_visionInform.yawPre *= 1.0;
				else if(fabs(s_visionInform.yawPre) > 600.0f && fabs(s_visionInform.yawPre) < 2000.0f) 
					s_visionInform.yawPre *= 0.95;
				else if(fabs(s_visionInform.yawPre) > 2000.0f && fabs(s_visionInform.yawPre) < 4000.0f) 
					s_visionInform.yawPre *= 0.7;
				else if(fabs(s_visionInform.yawPre) > 4000.0f && fabs(s_visionInform.yawPre) < 7000.0f) 
					s_visionInform.yawPre *= 0.6;
				else if(s_visionInform.yawPre >  7000.0f) s_visionInform.yawPre = 600;
				else if(s_visionInform.yawPre < -7000.0f) s_visionInform.yawPre = -600;
//				printf("%f\r\n", s_visionInform.yawPre);
				s_gimbal_yaw_motor.visTarget = s_visionInform.yawPos + s_visionInform.yawPre;
			}	
			
				lastX = pcdata->x.data;
				lastY = pcdata->y.data;
				if((pcdata->z.data) > 30.0f && (pcdata->z.data) < 700.0f)
				lastZ = pcdata->z.data;
				if((pcdata->z.data) > 700.0f) pcdata->z.data = 700.0f;
				Fps.pc++;
    }
//		else printf("CRC MISSING!\r\n");
		__HAL_UART_CLEAR_PEFLAG(&VISION_RX_USART_HANDLE);
}

static void VofaJF(Frame *vofaData, float a, float b)
{
				vofaData->fdata[0] = a;
				vofaData->fdata[1] = b;
				vofaData->tail[0] = 0x00;
				vofaData->tail[1] = 0x00;
				vofaData->tail[2] = 0x80;
				vofaData->tail[3] = 0x7f;
				HAL_UART_Transmit(&DEBUG_USART_HANDLE, (uint8_t *)&vofaData, sizeof(vofaData), 10);
}



